#include <avr/interrupt.h>
#include <compat/deprecated.h> 
#include <avr/sfr_defs.h>
#include <util/twi.h>

#include "i2c.h"


// Standard I2C bit rates are:
// 100KHz for slow speed
// 400KHz for high speed

// I2C state and address variables
static volatile eI2cStateType I2cState;
static volatile eI2cErrorType I2cError;
static volatile int I2cTimeOut;
static unsigned char I2cDeviceAddrRW;
static unsigned char I2cTranferMode;

// send/transmit buffer (outgoing data)
static unsigned char I2cSendData[I2C_SEND_DATA_BUFFER_SIZE];
static unsigned char I2cSendDataIndex;
static unsigned char I2cSendDataLength;
// receive buffer (incoming data)
static unsigned char I2cReceiveData[I2C_RECEIVE_DATA_BUFFER_SIZE];
static unsigned char I2cReceiveDataIndex;
static unsigned char I2cReceiveDataLength;

// function pointer to i2c receive routine
//! I2cSlaveReceive is called when this processor
// is addressed as a slave for writing
static void (*i2cSlaveReceive)(unsigned char receiveDataLength, unsigned char* recieveData);
//! I2cSlaveTransmit is called when this processor
// is addressed as a slave for reading
static unsigned char (*i2cSlaveTransmit)(unsigned char transmitDataLengthMax, unsigned char* transmitData);

// functions
void i2cInit(void)
{
    // set pull-up resistors on I2C bus pins
    sbi(PORTC, 0);  // i2c SCL on ATmega163,323,16,32,etc
    sbi(PORTC, 1);  // i2c SDA on ATmega163,323,16,32,etc
    //sbi(PORTD, 0);  // i2c SCL on ATmega128,64
    //sbi(PORTD, 1);  // i2c SDA on ATmega128,64

    // clear SlaveReceive and SlaveTransmit handler to null
    i2cSlaveReceive = 0;
    i2cSlaveTransmit = 0;
    I2cTranferMode = FALSE;
    // set i2c bit rate to 100KHz
    i2cSetBitrate(100);
    // enable TWI (two-wire interface)
    sbi(TWCR, TWEN);
    // set state
    I2cState = I2C_IDLE;
	I2cError = I2C_NONE;
    // enable TWI interrupt and slave address ACK
    sbi(TWCR, TWIE);
    sbi(TWCR, TWEA);
    //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
    // enable interrupts
    sei();
}

void i2cSetBitrate(unsigned int bitrateKHz)
{
    unsigned char bitrate_div;
    // set i2c bitrate
    // SCL freq = F_CPU/(16+2*TWBR))
    #ifdef TWPS0
        // for processors with additional bitrate division (mega128)
        // SCL freq = F_CPU/(16+2*TWBR*4^TWPS)
        // set TWPS to zero
        cbi(TWSR, TWPS0);
        cbi(TWSR, TWPS1);
    #endif
    // calculate bitrate division   
    bitrate_div = ((F_CPU/1000l)/bitrateKHz);
    if(bitrate_div >= 16)
        bitrate_div = (bitrate_div-16)/2;
    outb(TWBR, bitrate_div);
}

void i2cSetLocalDeviceAddr(unsigned char deviceAddr, unsigned char genCallEn)
{
    // set local device address (used in slave mode only)
    outb(TWAR, ((deviceAddr&0xFE) | (genCallEn?1:0)) );
}

void i2cSetSlaveReceiveHandler(void (*i2cSlaveRx_func)(unsigned char receiveDataLength, unsigned char* recieveData))
{
    i2cSlaveReceive = i2cSlaveRx_func;
}

void i2cSetSlaveTransmitHandler(unsigned char (*i2cSlaveTx_func)(unsigned char transmitDataLengthMax, unsigned char* transmitData))
{
    i2cSlaveTransmit = i2cSlaveTx_func;
}

inline void i2cSendStart(void)
{
    // send start condition
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTA));
}

inline void i2cSendStop(void)
{
    // transmit stop condition
    // leave with TWEA on for slave receiving
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA)|_BV(TWSTO));
}


inline int i2cWaitForComplete(void)
{
    // wait for i2c interface to complete operation
    while( !(inb(TWCR) & _BV(TWINT)) );
	return TRUE;

	// for(I2cTimeOut=0;I2cTimeOut<5;I2cTimeOut++)
	// {
		// if(!(inb(TWCR) & _BV(TWINT)))
		// {
			// return TRUE;
		// }
	// }
	// return FALSE;
}

inline void i2cSendByte(unsigned char data)
{
    // save data to the TWDR
    outb(TWDR, data);
    // begin send
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
}

inline void i2cReceiveByte(unsigned char ackFlag)
{
    // begin receive over i2c
    if( ackFlag )
    {
        // ackFlag = TRUE: ACK the recevied data
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
    }
    else
    {
        // ackFlag = FALSE: NACK the recevied data
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
    }
}

inline unsigned char i2cGetReceivedByte(void)
{
    // retieve received data byte from i2c TWDR
    return( inb(TWDR) );
}

inline unsigned char i2cGetStatus(void)
{
    // retieve current i2c status from i2c TWSR
    return( inb(TWSR) );
}

eI2cErrorType i2cGetError(void)
{
    // retieve current i2c error status
    return( I2cError );
}


void i2cMasterSend(unsigned char deviceAddr, unsigned char length, unsigned char* data)
{
    unsigned char i;
    // wait for interface to be ready
    while(I2cState);
    // set state
    I2cState = I2C_MASTER_TX;
    // save data
    I2cDeviceAddrRW = (deviceAddr & 0xFE);  // RW cleared: write operation
    for(i=0; i<length; i++)
        I2cSendData[i] = *data++;
    I2cSendDataIndex = 0;
    I2cSendDataLength = length;
    // send start condition
    i2cSendStart();
}

void i2cMasterReceive(unsigned char deviceAddr, unsigned char length, unsigned char* data)
{
    unsigned char i;
    // wait for interface to be ready
    while(I2cState);
    // set state
    I2cState = I2C_MASTER_RX;
    // save data
    I2cDeviceAddrRW = (deviceAddr|0x01);    // RW set: read operation
    I2cReceiveDataIndex = 0;
    I2cReceiveDataLength = length;
    // send start condition
    i2cSendStart();
    // wait for data
    while(I2cState);
    // return data
    for(i=0; i<length; i++)
        *data++ = I2cReceiveData[i];
}


void i2cMasterTransfer(unsigned char deviceAddr, 
                            unsigned char sendlength, unsigned char* senddata, 
                            unsigned char receivelength, unsigned char* receivedata)
{
    unsigned char i;
    // wait for interface to be ready
    while(I2cState != I2C_IDLE);

    // set Tranfer Mode
    I2cTranferMode = 1;

    // set state
    I2cState = I2C_MASTER_TX;

    // save data
    I2cDeviceAddrRW = (deviceAddr & 0xFE);  // RW cleared: write operation
    for(i=0; i<sendlength; i++)
        I2cSendData[i] = *senddata++;

    I2cSendDataIndex = 0;
    I2cSendDataLength = sendlength;
	

    I2cReceiveDataIndex = 0;
    I2cReceiveDataLength = receivelength;
    // send start condition
    i2cSendStart();

    // wait for data
    while(I2cState != I2C_IDLE);
	
    // return data
    for(i=0; i<receivelength; i++)
        *receivedata++ = I2cReceiveData[i];


    // Clear Tranfer Mode
//    I2cTranferMode = 0;
		
}


void i2cMasterSendNI(unsigned char deviceAddr, unsigned char length, unsigned char* data)
{
    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    i2cSendStart();
    
	if( i2cWaitForComplete() == FALSE )
	{		
		I2cError = I2C_BUS_ERROR;	
		sbi(TWCR, TWIE);
		return;
	}
	
    // send device address with write
    i2cSendByte( deviceAddr & 0xFE );
	if( i2cWaitForComplete() == FALSE )
	{
		I2cError = I2C_BUS_ERROR;	
		sbi(TWCR, TWIE);
		return;
	}
    
    // send data
    while(length)
    {
        i2cSendByte( *data++ );
        i2cWaitForComplete();
        length--;
    }
    
    // transmit stop condition
    // leave with TWEA on for slave receiving
    i2cSendStop();
    //@TH while( !(inb(TWCR) & _BV(TWSTO)) );

    // enable TWI interrupt
    sbi(TWCR, TWIE);
}


void i2cMasterReceiveNI(unsigned char deviceAddr, unsigned char length, unsigned char *data)
{
    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    i2cSendStart();
    i2cWaitForComplete();

    // send device address with read
    i2cSendByte( deviceAddr | 0x01 );
    i2cWaitForComplete();

    // accept receive data and ack it
    while(length > 1)
    {
        i2cReceiveByte(TRUE);
        i2cWaitForComplete();
        *data++ = i2cGetReceivedByte();
        // decrement length
        length--;
    }

    // accept receive data and nack it (last-byte signal)
    i2cReceiveByte(TRUE);
    i2cWaitForComplete();
    *data++ = i2cGetReceivedByte();

    // transmit stop condition
    // leave with TWEA on for slave receiving
    i2cSendStop();

    // enable TWI interrupt
    sbi(TWCR, TWIE);
}

void i2cMasterTransferNI(unsigned char deviceAddr, unsigned char sendlength, unsigned char* senddata, unsigned char receivelength, unsigned char* receivedata)
{
    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    i2cSendStart();
    i2cWaitForComplete();

	if( i2cWaitForComplete() == FALSE )
	{
		I2cError = I2C_BUS_ERROR;	
		sbi(TWCR, TWIE);
		return;
	}
	
	
    // if there's data to be sent, do it
    if(sendlength)
    {
        // send device address with write
        i2cSendByte( deviceAddr & 0xFE );
       	if( i2cWaitForComplete() == FALSE )
		{
			I2cError = I2C_BUS_ERROR;	
			sbi(TWCR, TWIE);
			return;
		}

        
        // send data
        while(sendlength)
        {
            i2cSendByte( *senddata++ );
            i2cWaitForComplete();
            sendlength--;
        }
    }

    // if there's data to be received, do it
    if(receivelength)
    {
        // send repeated start condition
        i2cSendStart();
        i2cWaitForComplete();

        // send device address with read
        i2cSendByte( deviceAddr | 0x01 );
        i2cWaitForComplete();

        // accept receive data and ack it
        while(receivelength > 1)
        {
            i2cReceiveByte(TRUE);
            i2cWaitForComplete();
            *receivedata++ = i2cGetReceivedByte();
            // decrement length
            receivelength--;
        }

        // accept receive data and nack it (last-byte signal)
        //i2cReceiveByte(TRUE);
        i2cReceiveByte(FALSE);
        i2cWaitForComplete();
        *receivedata++ = i2cGetReceivedByte();
    }
    
    // transmit stop condition
    // leave with TWEA on for slave receiving
    i2cSendStop();
    //while( !(inb(TWCR) & BV(TWSTO)) );

    // enable TWI interrupt
    sbi(TWCR, TWIE);
}

//! I2C (TWI) interrupt service routine
ISR(TWI_vect)
{
    //IntProlog();
    //DISABLE_SEND_ISR;			// Disable UDRE interrupt

    // read status bits
    unsigned char status = inb(TWSR) & TWSR_STATUS_MASK;

    switch(status)
    {
    // Master General
    case TW_START:                      // 0x08: Sent start condition
    case TW_REP_START:                  // 0x10: Sent repeated start condition
        // send device address
        i2cSendByte(I2cDeviceAddrRW);
        //outb(TWDR, I2cDeviceAddrRW);
        //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        break;
    
    // Master Transmitter & Receiver status codes
    case TW_MT_SLA_ACK:                 // 0x18: Slave address acknowledged
    case TW_MT_DATA_ACK:                // 0x28: Data acknowledged
        if(I2cSendDataIndex < I2cSendDataLength)
        {
            // send data
            i2cSendByte( I2cSendData[I2cSendDataIndex++] );
            //outb(TWDR, I2cSendData[I2cSendDataIndex++]);
            //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        }
        
        else if(I2cTranferMode == 1)
        {
            // send device address with read
            I2cDeviceAddrRW |= 0x01;

            // send repeated start condition
            i2cSendStart();
        }

        else
        {
            // transmit stop condition, enable SLA ACK
            i2cSendStop();
            //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTO)|_BV(TWEA));
            // set state
            I2cState = I2C_IDLE;
			I2cError = I2C_NONE;
        }
        break;
    case TW_MR_DATA_NACK:               // 0x58: Data received, NACK reply issued
        // store final received data byte
        I2cReceiveData[I2cReceiveDataIndex++] = inb(TWDR);
        // continue to transmit STOP condition
    case TW_MR_SLA_NACK:                // 0x48: Slave address not acknowledged
    case TW_MT_SLA_NACK:                // 0x20: Slave address not acknowledged
    case TW_MT_DATA_NACK:               // 0x30: Data not acknowledged
        // transmit stop condition, enable SLA ACK
        i2cSendStop();
        //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTO)|_BV(TWEA));
        // set state
        I2cState = I2C_IDLE;
		I2cError = I2C_NACK;
        break;
    case TW_MT_ARB_LOST:                // 0x38: Bus arbitration lost
    //case TW_MR_ARB_LOST:              // 0x38: Bus arbitration lost
        // release bus
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        // set state
        I2cState = I2C_IDLE;
		I2cError = I2C_ARB_LOST;
        // release bus and transmit start when bus is free
        //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTA));
        break;
    case TW_MR_DATA_ACK:                // 0x50: Data acknowledged
        // store received data byte
        I2cReceiveData[I2cReceiveDataIndex++] = inb(TWDR);
        // fall-through to see if more bytes will be received
    case TW_MR_SLA_ACK:                 // 0x40: Slave address acknowledged
        if(I2cReceiveDataIndex < (I2cReceiveDataLength-1))
            // data byte will be received, reply with ACK (more bytes in transfer)
            i2cReceiveByte(TRUE);
            //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        else
            // data byte will be received, reply with NACK (final byte in transfer)
            i2cReceiveByte(FALSE);
            //outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        break;

    // Slave Receiver status codes
    case TW_SR_SLA_ACK:                 // 0x60: own SLA+W has been received, ACK has been returned
    case TW_SR_ARB_LOST_SLA_ACK:        // 0x68: own SLA+W has been received, ACK has been returned
    case TW_SR_GCALL_ACK:               // 0x70:     GCA+W has been received, ACK has been returned
    case TW_SR_ARB_LOST_GCALL_ACK:      // 0x78:     GCA+W has been received, ACK has been returned
        // we are being addressed as slave for writing (data will be received from master)
        // set state
        I2cState = I2C_SLAVE_RX;
        // prepare buffer
        I2cReceiveDataIndex = 0;
        // receive data byte and return ACK
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        break;
    case TW_SR_DATA_ACK:                // 0x80: data byte has been received, ACK has been returned
    case TW_SR_GCALL_DATA_ACK:          // 0x90: data byte has been received, ACK has been returned
        // get previously received data byte
        I2cReceiveData[I2cReceiveDataIndex++] = inb(TWDR);
        // check receive buffer status
        if(I2cReceiveDataIndex < I2C_RECEIVE_DATA_BUFFER_SIZE)
        {
            // receive data byte and return ACK
            outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        }
        else
        {
            // receive data byte and return NACK
            outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        }
        break;
    case TW_SR_DATA_NACK:               // 0x88: data byte has been received, NACK has been returned
    case TW_SR_GCALL_DATA_NACK:         // 0x98: data byte has been received, NACK has been returned
        // receive data byte and return NACK
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        break;
    case TW_SR_STOP:                    // 0xA0: STOP or REPEATED START has been received while addressed as slave
        // switch to SR mode with SLA ACK
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        // i2c receive is complete, call i2cSlaveReceive
        if(i2cSlaveReceive) i2cSlaveReceive(I2cReceiveDataIndex, I2cReceiveData);
        // set state
        I2cState = I2C_IDLE;
		I2cError = I2C_SR_STOP;
        break;

    // Slave Transmitter
    case TW_ST_SLA_ACK:                 // 0xA8: own SLA+R has been received, ACK has been returned
    case TW_ST_ARB_LOST_SLA_ACK:        // 0xB0:     GCA+R has been received, ACK has been returned
        // we are being addressed as slave for reading (data must be transmitted back to master)
        // set state
        I2cState = I2C_SLAVE_TX;
        // request data from application
        if(i2cSlaveTransmit) I2cSendDataLength = i2cSlaveTransmit(I2C_SEND_DATA_BUFFER_SIZE, I2cSendData);
        // reset data index
        I2cSendDataIndex = 0;
        // fall-through to transmit first data byte
    case TW_ST_DATA_ACK:                // 0xB8: data byte has been transmitted, ACK has been received
        // transmit data byte
        outb(TWDR, I2cSendData[I2cSendDataIndex++]);
        if(I2cSendDataIndex < I2cSendDataLength)
            // expect ACK to data byte
            outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        else
            // expect NACK to data byte
            outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        break;
    case TW_ST_DATA_NACK:               // 0xC0: data byte has been transmitted, NACK has been received
    case TW_ST_LAST_DATA:               // 0xC8:
        // all done
        // switch to open slave
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        // set state
        I2cState = I2C_IDLE;
		I2cError = I2C_NACK;
        break;

    // Misc
    case TW_NO_INFO:                    // 0xF8: No relevant state information
        // do nothing
        break;
    case TW_BUS_ERROR:                  // 0x00: Bus error due to illegal start or stop condition
        // reset internal hardware and release bus
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTO)|_BV(TWEA));
        // set state
        I2cState = I2C_IDLE;
		I2cError = I2C_BUS_ERROR;
        break;
    }
    //outb(PORTB, ~I2cState);
//	ENABLE_SEND_ISR;
	//Epilog();

}

/*
void i2cSendNI(unsigned char deviceAddr, unsigned char length, unsigned char* data)
{
    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTA));
    while( !(inb(TWCR) & _BV(TWINT)) );

    // send device address with write
    outb(TWDR, (deviceAddr&0xFE) );
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
    while( !(inb(TWCR) & _BV(TWINT)) );
    
    // send data
    while(length)
    {
        outb(TWDR, *data++);
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
        while( !(inb(TWCR) & _BV(TWINT)) );
        length--;
    }
    
    // transmit stop condition
    // leave with TWEA on for slave receiving
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA)|_BV(TWSTO));
    // enable TWI interrupt
    sbi(TWCR, TWIE);
}

void i2cReceiveNI(unsigned char deviceAddr, unsigned char length, unsigned char *data)
{
    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWSTA));
    while( !(inb(TWCR) & _BV(TWINT)) );

    // send device address with read
    outb(TWDR, (deviceAddr&0xFE) | 0x01);
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
    while( !(inb(TWCR) & _BV(TWINT)) );

    // accept receive data and ack it
    while(length > 1)
    {
        outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA));
        while( !(inb(TWCR) & _BV(TWINT)) );
        *data++ = inb(TWDR);
        length--;
    }

    // accept receive data and nack it (last-byte signal)
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT));
    while( !(inb(TWCR) & _BV(TWINT)) );
    *data++ = inb(TWDR);

    // transmit stop condition
    // leave with TWEA on for slave receiving
    outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|_BV(TWINT)|_BV(TWEA)|_BV(TWSTO));
    // enable TWI interrupt
    sbi(TWCR, TWIE);
}
*/
